#include <joyos.h>
#include <happylib.h>
#include "define.h"
#include "distance.h"

/**
 * getDistance gets the distance from the distance sensor at the given port
 * The unit of the calculated distance is in inches
 *
 * @requires none
 * @effects gets the distance from the detecting equpment and another object
 * @modifies none
 */
double getDistance(void){
	//double measured_distance = 0.0;
	double distance_sum = 0.0;
	
	for (uint8_t i = 0; i < 10; i++){
		//measured_distance = irdist_read (DISTANCE_PORT)/2.54;
		//distance_sum += measured_distance;
		distance_sum += irdist_read (DISTANCE_PORT)/2.54;
	}
	
	return (distance_sum + 0.0) / 10;
}

/**
 * isClose is the function that checks if the distance from the distance sensor and its obstacles
 * The limit is as defined in define.h or umain().
 *
 * Returns 1 iff the distance from the Sharp Distance Sensor to the object is above the
 * defined threshold
 * Returns 0 otherwise
 *
 * @requires none
 * @effects Return
 */
uint8_t isClose(void){
	double distance = getDistance();
	if (distance < DISTANCE_THRESHOLD)
		return 1;
	else
		return 0;
}
